#pragma once
#include <ros/ros.h>
#include "common_lib/common.h"
#include "control/custom_class.h"
#include <tf/tf.h>
#include "amr_trajectory_msgs/amr_trajectory_msg.h"

#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

namespace control
{
    class TrajectoryProcessBase
    {
    public:
        int last_min_index_ = -1;
    public:
        virtual ~TrajectoryProcessBase(){};
        virtual void Init(ros::NodeHandle n) = 0;
        virtual void GlobalToVehiclePos(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state)
        {
            Eigen::Vector2d r;
            r(0) = amr_state.amr_local_.pose.position.x;
            r(1) = amr_state.amr_local_.pose.position.y;
            Eigen::Matrix2d R;
            float loc_theta = tf::getYaw(amr_state.amr_local_.pose.orientation);
            R(0, 0) = cos(loc_theta);
            R(0, 1) = -sin(loc_theta);
            R(1, 0) = sin(loc_theta);
            R(1, 1) = cos(loc_theta);
            Eigen::Matrix3d T;
            T(0, 0) = R(0, 0);
            T(0, 1) = R(0, 1);
            T(0, 2) = r(0);
            T(1, 0) = R(1, 0);
            T(1, 1) = R(1, 1);
            T(1, 2) = r(1);
            T(2, 0) = 0;
            T(2, 1) = 0;
            T(2, 2) = 1;
            Eigen::Vector3d pit;
            Eigen::Matrix3d T_inv = T.inverse();
            // int i = 0;
            for (auto &p : trajectory.points)
            {
                pit(0) = p.pose.position.x;
                pit(1) = p.pose.position.y;
                pit(2) = 1;
                Eigen::Vector3d temp = T_inv * pit;
                p.loc_pose.position.x = temp(0);
                p.loc_pose.position.y = temp(1);

                p.loc_pose.orientation = tf::createQuaternionMsgFromYaw(common_cal_.AngleConstraintToPI(tf::getYaw(p.pose.orientation) - loc_theta));
            }
        }

        virtual amr_trajectory_msgs::point_info SearchAllMapNearestPoint(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state) = 0;
        virtual amr_trajectory_msgs::amr_trajectory_msg SearchOnlinePoint(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state)
        {
            amr_trajectory_msgs::amr_trajectory_msg path_pick_up;
            return path_pick_up;
        };
        virtual amr_trajectory_msgs::amr_trajectory_msg TrajectoryProcess(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state, float pick_up_length, long int &last_min_index)
        {
            last_min_index_ = last_min_index;
    
            int min_index = SearchNearestPoint(trajectory, amr_state);
            if (!amr_state.actuator.is_auto_mode)
            {
                last_min_index = -1;
                last_min_index_ = -1;
            }

            if (min_index < 0 && min_index < last_min_index && last_min_index > -1)
            {
                min_index = last_min_index;
            }
            else if(min_index > last_min_index)
            {
                last_min_index = min_index;
            }
            else
            {
                if (last_min_index < 0 && !trajectory.points.empty())
                {
                    last_min_index = 0;
                    min_index = 0;
                }
            }
            last_min_index_ = last_min_index;

            amr_trajectory_msgs::amr_trajectory_msg path_pro = PickUpPath(trajectory, amr_state, min_index, pick_up_length);
            // LOG_INFO("min index: {}", min_index);
            GlobalToVehiclePos(path_pro, amr_state);
            return path_pro;
        }
        virtual int SearchNearestPoint(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state) = 0;
        virtual amr_trajectory_msgs::amr_trajectory_msg PickUpPath(amr_trajectory_msgs::amr_trajectory_msg &path, AMRState &amr_state, int min_index, float pick_up_length) = 0;

    private:
        amr_trajectory_msgs::amr_trajectory_msg online_point_;
        common_lib::Common common_cal_;

    };
} // namespace control
